/*
#include <Motion.h>
#include <Motion.h>
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-11-27     Fpi_lc       the first version
 */

/*
 * Author's Words:
 *  Motion control is the core of automation.Common motion
 *  models and entry functions are built here.
 *  Use c++ object-oriented design method to manage the
 *  motor driver object. It's easier to understand and
 *  program.
 */


#include "Motion.h"

#include <rtthread.h>
#include "ulog.h"

TmcDriver tmc_driver0(0);

TmcDriver tmc_driver1(1);
TmcDriver tmc_driver2(2);
TmcDriver tmc_driver3(3);
TmcDriver tmc_driver4(4);
TmcDriver tmc_driver5(5);
TmcDriver tmc_driver6(6);

TmcDriver *tmc_driver[7] =
{
        &tmc_driver0,
        &tmc_driver1,
        &tmc_driver2,
        &tmc_driver3,
        &tmc_driver4,
        &tmc_driver5,
        &tmc_driver6,
};

/*定义一个函数指针,用于索引 */
void (*Motor_D1_Task_Ptr[7]) (void *parameter) =
{
    Motor_D0_Task,
    Motor_D1_Task,
    Motor_D2_Task,
    Motor_D3_Task,
    Motor_D4_Task,
    Motor_D5_Task,
    Motor_D6_Task,
};


/* rt-os 任务入口必须为静态函数 做一次封装 */
void Motor_D0_Task(void *parameter){    while(1)    {        tmc_driver[0]->run_task(parameter);    }}
void Motor_D1_Task(void *parameter){    while(1)    {        tmc_driver[1]->run_task(parameter);    }}
void Motor_D2_Task(void *parameter){    while(1)    {        tmc_driver[2]->run_task(parameter);    }}
void Motor_D3_Task(void *parameter){    while(1)    {        tmc_driver[3]->run_task(parameter);    }}
void Motor_D4_Task(void *parameter){    while(1)    {        tmc_driver[4]->run_task(parameter);    }}
void Motor_D5_Task(void *parameter){    while(1)    {        tmc_driver[5]->run_task(parameter);    }}
void Motor_D6_Task(void *parameter){    while(1)    {        tmc_driver[6]->run_task(parameter);    }}


// The constructor
TmcDriver::TmcDriver(rt_uint8_t _driver_num)
{
    driver_num = _driver_num;
}
// The destructor
TmcDriver::~TmcDriver()
{


}


rt_uint32_t TmcDriver::init()
{
    return TMC5160_init(driver_num);
}


rt_uint32_t TmcDriver::getRunMode(void)
{
    return run_param.rampmode;
}

//rt_uint32_t TmcDriver::setRunMode(rt_uint8_t runMode)
//{
//    run_param.rampmode = runMode;
//}

rt_uint32_t TmcDriver::startup(void)
{

return 1;
}
//Move to target position
rt_uint32_t TmcDriver::moveToPositon(rt_int32_t position)
{
    return moveTo(driver_num, position);
}

/*
 * Brief:
 * Name:
 * Parameter: velocity units are rps.
 * Author:
 * Date:
 */

rt_uint32_t TmcDriver::moveByVelocity(float32_t f_velocity)
{
    int32_t l_velocity = f_velocity * 200 * 256;
    return rotate(driver_num, l_velocity);
}

//Moving to the target position at a certain velocity
/*
 * Generally no acceleration is specified, so the acceleration
 * is assigned as 4 times the velocity, which can be modified
 * by macro definition.
 */
rt_uint32_t TmcDriver::moveToPosByVelocity(rt_int32_t position, float32_t velocity)
{
    /*
     *
     *
     */

    return 1;
}

//Motor go-home function
/*
 * More advanced reset operation can eliminate mechanical clearance.
 */

rt_uint32_t TmcDriver::goHome(GoHome_Mode mode)
{

    return 1;
}
/*
 * Sometimes you need to specify reset speed.
 */
rt_uint32_t TmcDriver::goHomeByVelocity(float32_t velocity)
{
    return 1;
}


//User - defined complex functions, such as needle level detection
rt_uint8_t TmcDriver::positionCheck(void)
{
    GAP(TYPE_ID_ACTUAL_POSITION,driver_num,&run_param.x_actual);
    if(run_param.x_actual == run_param.x_target)
    {
        return true;
    }
    return false;
}

rt_uint8_t TmcDriver::positionReached(void)
{
    int32_t flag;
    GAP(TYPE_ID_POSITION_REACHED_FLAG,driver_num,&flag);
    if(flag == 1)
    {
        return true;
    }
    return false;
}

rt_uint8_t TmcDriver::velocityReached(void)
{
    GAP(TYPE_ID_ACTUAL_POSITION,driver_num,&run_param.x_actual);
    if(run_param.x_actual == run_param.x_target)
    {
        return true;
    }
    return false;
}

rt_uint8_t TmcDriver::stopReached(void)
{
    GAP(TYPE_ID_ACTUAL_POSITION,driver_num,&run_param.x_actual);
    if(run_param.x_actual == run_param.x_target)
    {
        return true;
    }
    return false;
}

rt_uint8_t TmcDriver::getCurPosition(void)
{
    GAP(TYPE_ID_ACTUAL_POSITION,driver_num,&run_param.x_actual);
    if(run_param.x_actual == run_param.x_target)
    {
        return true;
    }
    return false;
}

rt_uint8_t TmcDriver::goHomeMinimAccCal(void)
{
    GAP(TYPE_ID_ACTUAL_POSITION,driver_num,&run_param.x_actual);
    if(run_param.x_actual == run_param.x_target)
    {
        return true;
    }
    return false;
}

rt_uint8_t TmcDriver::SetTargetPos(rt_int32_t position)
{
    run_param.x_target = position;
    run_param.rampmode = POSITION;
    return true;
}




// Automatic calibration of motor

/*
 * Loop tasks:
 * Using state machine design in bare metal development,
 * Allocates priorities and time slices in the operating system.
 */


void TmcDriver::gohome_task(void *parameter)
{
    rt_err_t result;
    rt_int32_t position;

    result = rt_sem_take(sem_motor[driver_num].goHome_start,RT_WAITING_FOREVER);

    if(result != RT_EOK)
    {
        LOG_E("rt_sem_take sem_m1_gohomestart failed...\n");
    }
    position = goHomeMinimAccCal();
    moveToPositon(position);
    while(positionReached() == false)
    {
        rt_thread_mdelay(10);
    }
    //goHomeStage1();
    while(stopReached() == false)
    {
        rt_thread_mdelay(10);
    }
    position = getCurPosition();
    moveToPositon(position);
    while(positionReached() == false)
    {
        rt_thread_mdelay(10);
    }
    rt_sem_release(sem_motor[driver_num].goHome_finished);
    return;
}

/*
* Function name: run_task()
* Input argv:
*      void
* Output argv:
*      void
* Return:
       void
* Function Description:  the destructor
* Version: 1.0
* Be careful:
*/
rt_event_t MotorD1Start;

void TmcDriver::run_task(void *parameter)
{

    static rt_uint32_t count;

    rt_sem_take(sem_motor[driver_num].RunStart,RT_WAITING_FOREVER);

    //test part
    rt_thread_mdelay(3000);
    LOG_D(" md%d running...",driver_num);
    rt_thread_mdelay(3000);
    LOG_D(" md%d running...",driver_num);
    rt_thread_mdelay(3000);
    LOG_D(" md%d running...",driver_num);
    rt_thread_mdelay(3000);
    LOG_D(" md%d end...",driver_num);

    switch(getRunMode())
    {
         case POSITION:

             break;
         case VELOCITY_CW:
         case VELOCITY_ACW:

             break;
         default:break;
    }
    while(1)
    {
        switch(getRunMode())
        {
           case POSITION:
               if(1)//positionReached() == true)
               {
                   /* 等待电机停稳 */
                   rt_thread_mdelay(10);
                   rt_sem_release(sem_motor[driver_num].RunFinished);
                   return;
               }
               else if(count > 1000)
               {
                   LOG_E("run timeout...\n");
                   return;
               }
               else
               {
                   rt_thread_mdelay(10);
                   count++;
               }
               break;

           case VELOCITY_CW:
           case VELOCITY_ACW:
               if(positionReached() == true)
               {
                   rt_sem_release(sem_motor[driver_num].RunFinished);
                   return;
               }
               rt_thread_mdelay(10);
               count++;
               if(count > 1000)
               {
                   LOG_E("run timeout...\n");
                   return;
               }
               break;
           default:break;
        }
    }

}


void TmcDriver::liquidDet_task(void *parameter)
{
    rt_err_t result;
    rt_int32_t position;

    result = rt_sem_take(sem_motor[driver_num].liquidDet_start,RT_WAITING_FOREVER);

    if(result != RT_EOK)
    {
        LOG_E("rt_sem_take sem_m1_gohomestart failed...\n");
    }
    position = goHomeMinimAccCal();
    moveToPositon(position);
    while(positionReached() == false)
    {
        rt_thread_mdelay(10);
    }
    //goHomeStage1();
    while(stopReached() == false)
    {
        rt_thread_mdelay(10);
    }
    position = getCurPosition();
    moveToPositon(position);
    while(positionReached() == false)
    {
        rt_thread_mdelay(10);
    }
    rt_sem_release(sem_motor[driver_num].goHome_finished);
    return;
}



